package org.robotfish.learn;

import org.ejml.simple.SimpleMatrix;
import org.robotfish.geometry.TransformationArc;

public class RigidLinkFishMechanicalModel implements MechanicalModel {

	LinkData linkData[];
/*	double dt;
	int timestep;*/
	
	public RigidLinkFishMechanicalModel(LinkData linkData[]) {
		this.linkData = linkData;
	}
	
	public double getMass() {
		double m = 0;
		for (LinkData l:linkData) {
			m += l.m;
		}
		return m;
	}
	
	public double getMomentOfIntertia(RigidLinkFishState state) {
		return 0;
		
		
		
	}
	
	
	/**
	 * 
	 * @param current kala seisund (mõjuvad jõud samuti kuigi neid ei kasutata)
	 * @param velocityCOG velocity of Center Of Gravity - velocity is 3-vector of (vx, vy, w)
	 * @param fishMotion
	 * @return kala seisund järgmisel ajahetkel ja kalale mõjuvad jõud
	 */
	public RigidLinkFishState getNextState(RigidLinkFishState currentState, SimpleMatrix velocityCOG, FishMotion fishMotion) {
		
		
		return null;
	}



	@Override
	public SimpleMatrix getForceCOG(RigidLinkFishState state) {
		// TODO Auto-generated method stub
		return null;
	}

	/**
	 * position of Center Of Gravity - 3-vector of (x, y, angle) 
	 * @param state field linkPositionCOG0 contains positions of centers of links
	 * @return
	 */
	public SimpleMatrix getPositionCOG(RigidLinkFishState state) {
		return getPositionCOG(state.linkPositionCOG0);
	}

	/**
	 * position of Center Of Gravity - 3-vector of (x, y, angle) 
	 * @param linkPositionCOG0 positions of centers of links
	 * @return
	 */
	public SimpleMatrix getPositionCOG(SimpleMatrix linkPositionCOG0[]) {
		SimpleMatrix cog = new SimpleMatrix(3, 1);
		
		
		return cog;
	}

	@Override
	public double getCurvature(RigidLinkFishState state) {
		// TODO Auto-generated method stub
		return 0;
	}

	@Override
	public int getTimeStep(RigidLinkFishState state) {
		// TODO Auto-generated method stub
		return state.timeStep;
	}
	
	/**
	 * @return kala nina hetke asend {x, y, angle}
	 */
	public SimpleMatrix getNose(RigidLinkFishState state) {
		SimpleMatrix noseCOG = state.linkPositionCOG0[0]; 
		return noseCOG.plus(TransformationArc.tangentVector(noseCOG.get(2)).scale(-linkData[0].a));
	}
}

